/*
The MIT License
Copyright (c) 2019 Lehrstuhl Informatik 11 - RWTH Aachen University
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE

This file is part of embeddedRTPS.

Author: i11 - Embedded Software, RWTH Aachen University
*/

#include "lwip/sys.h"
#include "lwip/tcpip.h"
#include "rtps/entities/StatefulReader.h"
#include "rtps/messages/MessageFactory.h"
#include "rtps/utils/Lock.h"
#include "rtps/utils/Log.h"

#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
#include "rtps/utils/printutils.h"
#define SFR_LOG(...)                                                           \
	if (true) {                                                                  \
		printf("[StatefulReader %s] ", &sfReader->m_reader.m_attributes.topicName[0]);                \
		printf(__VA_ARGS__);                                                       \
		printf("\n");                                                              \
	}
#else
#define SFR_LOG(...) //
#endif
void rtps_StatefulReader_Construct(struct rtps_StatefulReader *sfReader)
{
	if (sfReader != NULL) {
		SFR_LOG("StatefulReader: Failed to construct.\n");
		return;
	}
	rtps_Reader_Construct(&(sfReader->m_reader));
	sfReader->m_callback = NULL;
	sfReader->m_callee = NULL;
}

void rtps_StatefulReader_Destruct(struct rtps_StatefulReader *sfReader)
{
	if (sfReader != NULL) {
		SFR_LOG("StatefulReader: Failed to construct.\n");
		return;
	}
	rtps_Reader_Destruct(&(sfReader->m_reader));
}

void rtps_StatefulReader_init(struct rtps_StatefulReader *sfReader,
			      const TopicData *attributes, rtps_UdpDriver *driver)
{
	if (sys_mutex_new(&sfReader->m_mutex) != ERR_OK) {
		SFR_LOG("StatefulReader: Failed to create mutex.\n");
		return;
	}
	rtps_Reader_Construct(&(sfReader->m_reader));
	sfReader->m_reader.m_bstateful = true;
	sfReader->m_callback = NULL;
	sfReader->m_callee = NULL;

	sfReader->m_reader.m_attributes = *attributes;
	sfReader->m_transport = driver;
	sfReader->m_packetInfo.srcPort = attributes->unicastLocator.port;
	sfReader->m_reader.m_is_initialized = true;
}

bool statefulReader_cmp_guidandsn(void *cacheChange, void *writerproxy)
{
	if ((rtps_Guid_t_operator_equal(&(((WriterProxy *)writerproxy)->remoteWriterGuid), 
			&(((ReaderCacheChange *)cacheChange)->writerGuid)))
		&&(rtps_SequenceNumber_t_operator_equal(&(((WriterProxy * )writerproxy)->expectedSN), 
			&((ReaderCacheChange *)cacheChange)->sn))) {
		rtps_SequenceNumber_t_operator_increase(&((WriterProxy *)writerproxy)->expectedSN);
		return true;
	}

	return false;
}

void rtps_StatefulReader_newChange(struct rtps_StatefulReader *sfReader,
				   const ReaderCacheChange *cacheChange)
{
	bool ret;

	if (sfReader->m_callback == NULL)
		return;

	rtps_Lock(&sfReader->m_mutex);
	ret = rtps_MemoryPool_find(sfReader->m_reader.m_proxies, statefulReader_cmp_guidandsn, 
		(ReaderCacheChange *)cacheChange);

	if (ret) {
		sfReader->m_callback(sfReader->m_callee, cacheChange);
		rtps_Unlock(&sfReader->m_mutex);
		return;
	}

	rtps_Unlock(&sfReader->m_mutex);

}

void rtps_StatefulReader_registerCallback(struct rtps_StatefulReader *sfReader,
		ddsReaderCallback_fp cb, void *callee)
{
	if (cb != NULL) {
		sfReader->m_callback = cb;
		sfReader->m_callee = callee; // It's okay if this is null
	} else {
		SFR_LOG("Passed callback is nullptr\n");
	}
}

bool rtps_StatefulReader_addNewMatchedWriter(
			struct rtps_StatefulReader *sfReader,
			const WriterProxy *newProxy)
{
#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
	SFR_LOG("New writer added with id: ");
	printGuid(newProxy->remoteWriterGuid);
	SFR_LOG("\n");
#endif
	return rtps_MemoryPool_add(sfReader->m_reader.m_proxies, (WriterProxy *)newProxy);
}

bool statefulReader_cmp_guid(void *guid, void *writerproxy)
{
	return rtps_Guid_t_operator_equal(&(((WriterProxy * )writerproxy)->remoteWriterGuid), guid);
}

void rtps_StatefulReader_removeWriter(struct rtps_StatefulReader *sfReader,
				      const Guid_t *guid)
{

	rtps_Lock(&sfReader->m_mutex);
	rtps_MemoryPool_remove(sfReader->m_reader.m_proxies,
			       statefulReader_cmp_guid, (Guid_t *)guid);
	rtps_Unlock(&sfReader->m_mutex);
}

bool statefulReader_cmp_guidPrefix(void *guidPrefix, void *writerproxy)
{
	return rtps_GuidPrefix_t_operator_equal(&(((WriterProxy * )writerproxy)->remoteWriterGuid.prefix), guidPrefix);
}

void rtps_StatefulReader_removeWriterOfParticipant(
			struct rtps_StatefulReader *sfReader, const GuidPrefix_t *guidPrefix)
{
	rtps_Lock(&sfReader->m_mutex);
	rtps_MemoryPool_remove(sfReader->m_reader.m_proxies,
			       statefulReader_cmp_guidPrefix, (GuidPrefix_t *)guidPrefix);
	rtps_Unlock(&sfReader->m_mutex);
}

struct StatefulReader_data {
	SubmessageHeartbeat *msg;
	GuidPrefix_t *sourceGuidPrefix;
	WriterProxy *writer;
};

bool StatefulReader_get_writer(void *data, void *writer)
{
	struct StatefulReader_data *pdata = (struct StatefulReader_data *)data;
	
	if (rtps_GuidPrefix_t_operator_equal(&(((WriterProxy * )writer)->remoteWriterGuid.prefix), 
			pdata->sourceGuidPrefix)
		&& rtps_EntityId_t_operator_equal(
			&(((WriterProxy *)writer)->remoteWriterGuid.entityId),
			&((pdata->msg)->writerId))) {
		pdata->writer = writer;
		return true;
	}

	return false;
}
bool rtps_StatefulReader_onNewHeartbeat(struct rtps_StatefulReader *sfReader,
		struct SubmessageHeartbeat *msg, const GuidPrefix_t *sourceGuidPrefix)
{
	rtps_Lock(&sfReader->m_mutex);
	rtps_PacketInfo info;
	WriterProxy *writer = NULL;
	struct StatefulReader_data data;
	bool ret;
	SequenceNumberSet set;

	data.msg = (struct SubmessageHeartbeat *)msg;
	data.sourceGuidPrefix = (GuidPrefix_t *)sourceGuidPrefix;
	data.writer = NULL;

	ret = rtps_MemoryPool_find(sfReader->m_reader.m_proxies, StatefulReader_get_writer, &data);
	if (!ret) {
#if SFR_VERBOSE && RTPS_GLOBAL_VERBOSE
		SFR_LOG("Ignore heartbeat. Couldn't find a matching "
			"writer with id:");
		printEntityId(msg->writerId);
		SFR_LOG("\n");
#endif
		rtps_Unlock(&sfReader->m_mutex);
		return false;
	}

	writer = data.writer;

	if (msg->count.value <= writer->hbCount.value) {
		SFR_LOG("Ignore heartbeat. Count too low.\n");
		rtps_Unlock(&sfReader->m_mutex);
		return false;
	}

	writer->hbCount.value = msg->count.value;
	info.srcPort = sfReader->m_packetInfo.srcPort;
	info.destAddr = rtps_Locator_getIp4Address(&(writer->remoteLocator));
	info.destPort = rtps_Locator_getLocatorPort(&(writer->remoteLocator));
	info.buffer = rtps_PBufWrapper_Construct(RTPS_PBW_BY_LENG, 0);
	rtps_MessageFactory_addHeader(info.buffer,
				      &(sfReader->m_reader.m_attributes.endpointGuid.prefix));
	set = rtps_WriterProxy_getMissing(writer, &(msg->firstSN), &(msg->lastSN));
	rtps_MessageFactory_addAckNack(info.buffer, &(msg->writerId), &(msg->readerId),
				       &set,
				       rtps_WriterProxy_getNextAckNackCount(writer), false);

	SFR_LOG("Sending acknack.\n");
	rtps_UdpDriver_sendPacket(sfReader->m_transport, &info);

	rtps_PBufWrapper_Destruct(info.buffer);
	rtps_Unlock(&sfReader->m_mutex);
	return true;
}

#undef SFR_VERBOSE
